#include "User_OPENMV_DataRecieve.h"
#include "DXDY_PID.h"
#include <stdio.h>  // 包含 sprintf 函数
#include <stdint.h> // 包含 int32_t 类型
#include <string.h>
u8 u3_data_reg[56];
u8 u3_state = IDLE;
u8 u3_len_= 0;
u8 u3_len_reg = 0;
u8 Rx[4];
u8 Rx1[2];
u8 Serial_RxFlag;
u8 RxState;
u8 Rx1State;
u8 pRxPacket;
u8 pRx1Packet;
u8 Get_Target_state = 0;
u8 Target_num1 = 0;
u8 Target_num2 = 0;
void User_OPENMV_DataRecieve(u8 data) //length = 8
{
    if (RxState == 0)
		{
			if (data == 0xA1)
			{
				RxState = 1;
				pRxPacket = 0;
			}
		}
		else if (RxState == 1)
		{
			Rx[pRxPacket] = data;
			pRxPacket ++;
			if (pRxPacket >= 4)
			{
				RxState = 2;
			}
		}
		else if (RxState == 2)
		{
			if (data == 0xB1)
			{
				RxState = 0;
				Serial_RxFlag = 1;
				Deal_With_XY_Data(Rx);
			}
		}
		
}



void User_Fand_DataRecieve(u8 data) //length = 2
{
    if (Rx1State == 0)
		{
			if (data == 0xC1)
			{
				Rx1State = 1;
				pRx1Packet = 0;
			}
		}
		else if (Rx1State == 1)
		{
			Rx1[pRx1Packet] = data;
			pRx1Packet ++;
			if (pRx1Packet > 1)
			{
				Rx1State = 2;
			}
		}
		else if (Rx1State == 2)
		{
			if (data == 0xD1)
			{
				Rx1State = 0;
				Get_Target_state = 1;
				Target_num1 = Rx1[0];
        Target_num2 = Rx1[1];
        USART_SendData(UART5,Get_Target_state);

			}
		}
		
}

 void	Deal_With_OPENMV_Data(u8 u3_func,u8 len,u8 *data)
 {
    switch (u3_func)
    {
    case 0x01:/*0x01 识别颜色与形状 */
        Identify_Color_Shape(data);
        
        break;
    case 0x02://精准定位返回xy坐标偏差
				Deal_With_XY_Data(data);
        break;
    }
 }


 void Identify_Color_Shape(u8 *data)
 {
    switch (data[1])
    {
        case RED:

            break;
        case BLUE:

            break;

    }
    switch (data[2])
    {
    case CIRCLE:
        break;
    case RECT:
        break;
    case TRIANGLE:
        break;
    }
 }



void Deal_With_XY_Data(uint8_t *data) {
    int32_t x = 0;
    int32_t y = 0;
    //char str[24]; // 足够大以存储格式化字符串

    // 使用类型转换和指针操作解包数据
    x = (0.375*data[3]);
    y = (0.375*data[2]);

    Set_Target_XY(x,y,20);

    // 将 x 和 y 转换为字符串
    //snprintf(str, sizeof(str), "X: %d, Y: %d", x, y);

    // 可以在这里使用str，例如打印或存储
    //AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,str);
}
